# Authors:  Joseph Durel, Anurodh Giri, Mauricio Torrejon, Jeremy Vatter 
# Assignment 2  # Date Last Modified: 24 October 2011 
#  
# Purpose: Does some hit and run manuevers while playing sounds.  Also moves
# around a wall whlie maintaining some distance
# 

import create
import time #Used to sleep functions to prevent overloading robot

robot = create.Create("/dev/tty.KeySerial1")
robot.toSafeMode()

#Instance Variables - Global sensor list and Option to test
option = 1

# Option 1
# Move Robot forward until he hits a bump.
# If left bump is hit, robot turns 90 deg clockwise and goes forward
# If right bump is hit, robot turns 90 deg counterclockwise and goes forward
# If both sensors are hit, robot spins 180 and continues forward
#
# Bonus:
# Make noise depending on whatt sensor is hit

# Option 2
# Move Robot forward along a wall to its right while
# maintaining the same distance from the wall
# If the wall is too far away, curve to the right a bit
# If the wall is to close, curve to the left a bit
# If the wall is in an acceptable range, go forward
#
# Bonus:
# Follow inside corners and outside turns

# Function to perform Option 1
def option1():
    while True:
        s = robot.sensors( [create.LEFT_BUMP, create.RIGHT_BUMP] )
        # If both sensors are hit, turns around and plays a tone
        if s[create.LEFT_BUMP] == 1 and s[create.RIGHT_BUMP] == 1:
            robot.turn(180)
            robot.playSong( [(60,8), (64,8), (67,8), (72,8)] ) #C Chord
            # If left sensor is hit, turns right and plays a tone
        elif s[create.LEFT_BUMP] == 1:
            robot.turn(-90)
            robot.playSong( [(62,8), (66,8), (69,8), (74,8)] ) #D Chord
        # If right sensor is hit, turns right and playsa  tone
        elif s[create.RIGHT_BUMP] == 1:
            robot.turn(90)
            robot.playSong( [(58,8), (62,8), (65,8), (70,8)] ) #A Chord
        time.sleep(0.02)
        robot.go(10, 0) #Moves forward at 5cm/s
        
#def calibrateDist():
#    print 'Calibrating distance (50 cm)...'
#    s = robot.sensors( create.WALL_IR_SENSOR )
#    ir_distance = s[create.WALL_IR_SENSOR]
#    return ( ir_distance / 50 )

# Function to perform Option 2
def option2():
    s = robot.sensors( create.WALL_SIGNAL )
    # Set the distance to follow the wall at to the beginning distance
    followdistance = s[ create.WALL_SIGNAL ]
    # print 'followdistance = ',followdistance
    # Go some distance before starting the follow loop
    robot.go(10,0)
    time.sleep(.5)
    while True:
        s = robot.sensors( [create.WALL_SIGNAL, create.LEFT_BUMP, create.RIGHT_BUMP] )
        # Handle inside corners
        if s[create.RIGHT_BUMP] == 1:
            # Push the bot forward a bit to see if it's running into a wall (ie in a corner)
            robot.move(1)
            s = robot.sensors( [create.WALL_SIGNAL, create.LEFT_BUMP, create.RIGHT_BUMP] )
            if s[create.RIGHT_BUMP] == 1 and s[create.LEFT_BUMP] == 1:
                # If both sensors activate, it's in a corner,
                # so move back ~1cm and rotate ~90 away from the wall
                robot.move(-1)
                robot.turn(95)
            else:
                # This handles any case where the distance checking didn't work,
                # and the bot sidled up to the wall, setting off the right bumper
                robot.move(-2)
                robot.turn(10)
                robot.move(2)
        # Even though WALL_SIGNAL does not scale linearly, a 50-point buffer is
        # added to either side; this actually works out, because 50 results in a
        # closer tolerance inside and a looser tolerance outside.
        elif s[ create.WALL_SIGNAL ] > ( followdistance + 50 ):
            robot.go( 10, 20 )
        elif s[ create.WALL_SIGNAL ] < ( followdistance - 50 ):
            if s[ create.WALL_SIGNAL ] < 15:
                # In case the wall becomes really far away
                # Move the turning radius forward past the corner:
                robot.go(10,0)
                time.sleep(1)
                # Move the robot around the corner, pivoting (pretty much)
                # on the right wheel
                robot.go( 26, -90 )
                time.sleep(1.5)
                # Now move straight forward a bit more
                robot.go(10,0)
            else:
                # Readjust towards the wall a bit
                robot.go( 10, -20 )
        else:
            # Go forward a bit
            robot.go(10)
            time.sleep(.25)
        time.sleep(.1)
                
#

if option == 1:
	option1()
elif option == 2:
	option2()